#include <stm32l4xx.h>

#include <linux/init.h>
#include <linux/clocksource.h>

static TIM_HandleTypeDef htim2;

void TIM2_IRQHandler(void)
{
    HAL_TIM_IRQHandler(&htim2);
}

static u64 stm32_cs_read_up(struct clocksource *c)
{
    return __HAL_TIM_GET_COUNTER(&htim2);
}

static struct clocksource clksrc = {
    .name = "timer_cs",
    .mask = CLOCKSOURCE_MASK(32),
    .read = stm32_cs_read_up,
    .flags = CLOCK_SOURCE_IS_CONTINUOUS,
};

/* APBx timer clocks frequency doubler state related to APB1CLKDivider value */
static void pclkx_doubler_get(uint32_t *pclk1_doubler, uint32_t *pclk2_doubler)
{
    uint32_t flatency = 0;
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &flatency);

    *pclk1_doubler = 1;
    *pclk2_doubler = 1;

#if defined(SOC_SERIES_STM32MP1)
    if (RCC_ClkInitStruct.APB1_Div != RCC_APB1_DIV1)
    {
        *pclk1_doubler = 2;
    }
    if (RCC_ClkInitStruct.APB2_Div != RCC_APB2_DIV1)
    {
        *pclk2_doubler = 2;
    }
#else
    if (RCC_ClkInitStruct.APB1CLKDivider != RCC_HCLK_DIV1)
    {
        *pclk1_doubler = 2;
    }
#if !defined(SOC_SERIES_STM32F0) && !defined(SOC_SERIES_STM32G0)
    if (RCC_ClkInitStruct.APB2CLKDivider != RCC_HCLK_DIV1)
    {
        *pclk2_doubler = 2;
    }
#endif
#endif
}

static int timer_cs_init(void)
{
    uint32_t pclk1_doubler, pclk2_doubler;
    uint32_t clk, freq = 1000000;

    pclkx_doubler_get(&pclk1_doubler, &pclk2_doubler);

    clk = HAL_RCC_GetPCLK1Freq() * pclk1_doubler;

    htim2.Instance = TIM2;
    htim2.Init.Prescaler = clk / freq - 1;
    htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim2.Init.Period = 0xffffffff;
    htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
    htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;

    HAL_TIM_Base_Init(&htim2);
    __HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_UPDATE);

    clocksource_register_hz(&clksrc, freq);

    HAL_TIM_Base_Start(&htim2);
    // HAL_NVIC_EnableIRQ(TIM2_IRQn);

    return 0;
}
subsys_initcall(timer_cs_init);
